#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <taskLib.h>
#include <time.h>

#include "Api7841.h"
#include "Commdef.h"

int g_iIdCounter;
int g_iDataCounter;

static void DumpCanPacket(int iCardNo, int iPortNo, CAN_PACKET *pPacket)
{
	int i;
	printf("CAN Message Dumping, CardNo = %d, PortNo = %d\n", iCardNo, iPortNo);
	printf("\tCAN ID: %08X\n", pPacket->canId);
	printf("\tRTR: %d\n", pPacket->rtr);
	printf("\tLength: %d\n", pPacket->len);
	printf("\tData: ");
	for (i = 0; i < 8; ++i)
	{
		printf("%02X", pPacket->data[i]);
	}
	printf("\n\n");
}

void timer_write_handler(timer_t timerId, int arg)
{
	int i;
	int iRetVal;
	int iHandle = arg;
	CAN_PACKET sndPacket;
	sndPacket.rtr = 2;
	sndPacket.len = 8;
	sndPacket.canId = g_iIdCounter++;
	for (i = 0; i < 8; ++i)
	{
		sndPacket.data[i] = g_iDataCounter++;
	}
	
	iRetVal = CanSendMsg(iHandle, &sndPacket);
	if (iRetVal == -1)
	{
		printf("CAN Write Failed\n");
	}
}


int init_write_timer(int iHandle,int iPeriod)
{
	int iRetval;
	timer_t timerId;
	struct itimerspec timer_spec;
	iRetval = timer_create(CLOCK_REALTIME, NULL, &timerId);
	if (iRetval == ERROR)
	{
		printf("timer create failed\n");
		return -1;
	}
	iRetval = timer_connect(timerId, timer_write_handler, iHandle);
	
	if (iRetval == ERROR)
	{
		printf("timer connect failed\n");
		return -1;
	}
	timer_spec.it_value.tv_sec = 1;
	timer_spec.it_value.tv_nsec = 0;
	timer_spec.it_interval.tv_sec = 0;
	timer_spec.it_interval.tv_nsec = iPeriod * 1000 * 1000;
	iRetval = timer_settime(timerId, 0, &timer_spec, NULL);
	if (iRetval == ERROR)
	{
		printf("set timer failed\n");
		return -1;
	}

	return 0;
}

void CanWriteTask(int iCardNo, int iPortNo, int iPeriod)
{
	int iHandle = CanOpenDriver(iCardNo, iPortNo);
	if (iHandle == -1)
	{
		printf("\n CAN Open Failed, CardNo = %d, PortNo = %d\n", iCardNo, iPortNo);
	}
	CanClearTxBuffer(iHandle);
	init_write_timer(iHandle, iPeriod);
	while (1)
	{
		taskDelay(30);
	}
}
void CanReadTask(int iCardNo, int iPortNo)
{
	CAN_PACKET recvPacket;
	int iRecvCnt, i, iRetVal;
	int iHandle = CanOpenDriver(iCardNo, iPortNo);
	if (iHandle == -1)
	{
		printf("\n CAN Open Failed, CardNo = %d, PortNo = %d\n", iCardNo, iPortNo);
		return;
	}
	CanClearTxBuffer(iHandle);

	while (1)
	{
		iRecvCnt = CanGetRcvCnt(iHandle);
		for (i = 0; i < iRecvCnt; ++i)
		{
			iRetVal = CanRcvMsg(iHandle, &recvPacket);
			if (iRetVal != -1)
			{
				DumpCanPacket(iCardNo, iPortNo, &recvPacket);
			}
			else
			{
				printf("CAN Read Message Failed, CardNo = %d, PortNo = %d\n", iCardNo, iPortNo);
				break;
			}
		}
		if (iRecvCnt == 0)
		{
			taskDelay(10);
		}
	}
}
void StartCanReadTask(int iCardNo, int iPortNo)
{
	taskSpawn("tCanWrite", 50, 0, 4096, (FUNCPTR)CanReadTask, iCardNo, iPortNo, 0, 0, 0, 0, 0, 0, 0, 0);
}

void StartCanWriteTask(int iCardNo, int iPortNo, int iPeriod)
{
	taskSpawn("tCanRead", 50, 0, 4096, (FUNCPTR)CanWriteTask, iCardNo, iPortNo, iPeriod, 0, 0, 0, 0, 0, 0, 0);
}
